#include <rtthread.h>
#include "./Modbus/RTU/Master/modbusrtu_master.h"
#include "./Indication.h"
#include "./Move_Control.h"
#include "./Avoid.h"
#include "./Modbus/modbus_slave_core.h"

#define LOG_TAG "indication" /* 该模块TAG */
#define LOG_LVL LOG_LVL_DBG  /* 静态过滤级别为调试级 */
#include <ulog.h>            /* 必须放在宏定义下面 */

static void main_thread(void *parameter);

static struct rt_thread _thread;
ALIGN(RT_ALIGN_SIZE)
static char _thread_stack[1024];

#define MUSIC_ADDR 0x0010  /* 音乐播放设备寄存器地址 */
#define ALARM_ADDR 0x0011  /* 警示灯播放设备寄存器地址 */
#define OUTPUT_ADDR 0x0012 /* 输出指示播放设备寄存器地址 */

#define OUTPUT_OFF 0 /* 复位 */
#define OUTPUT_ON 1  /* 置位 */

static enum Sensor_Enum avoid[4]; /* 0~3对应前后左右4个避障 */

static enum Music {
    Music_Null = 0,        /* 不播放音乐 */
    Music_Power_On = 1,    /* 上电提示 */
    Music_Avoid_Check = 3, /* 障碍物提示 */
    Music_Normal = 0,      /* 正常行走提示音 */
} music = Music_Power_On;

static enum Alarm {
    Alarm_IDLE = 0, /* 空闲 */
    Alarm_Norm,     /* 正常行走 */
    Alarm_Warning,  /* 警告 */
    Alarm_Error,    /* 错误 */
} alarm = Alarm_IDLE;

static char slave_addr = 0;                            /* 从机地址 */
static unsigned short output = OUTPUT_OFF;             /* 输出指示 */
static enum AGV_Move_State_Enum agv_state = Move_IDLE; /* AGV运动状态 */
static enum Sensor_Enum sensor_state = Sensor_Normal;  /* 避障传感器状态 */

static void Update(void); /* 根据AGV运动状态，避障传感器状态更新音乐，警示灯，输出指示 */

void Indicate_Init(char addr)
{
    rt_err_t rt_result;
    slave_addr = addr;

    rt_result = rt_thread_init(&_thread, "indication thread", main_thread, RT_NULL, _thread_stack, sizeof(_thread_stack), 14, 2);
    if (rt_result == RT_EOK)
    {
        rt_thread_startup(&_thread); /* 启动线程 */
    }
    else
    {
        rt_kprintf("indication thread error\n");
    }
}

void main_thread(void *parameter)
{
    MR_Writer_Holding_Register(slave_addr, ALARM_ADDR, alarm, RT_WAITING_FOREVER);                          /* 设置警示灯 */
    MR_Writer_Holding_Register(slave_addr, MUSIC_ADDR, (unsigned short)Music_Power_On, RT_WAITING_FOREVER); /* 设置音乐 */

    // MR_Writer_Holding_Register(slave_addr, OUTPUT_ADDR, output, RT_WAITING_FOREVER); /* 设置输出 */

    for (int i = 0; i < 35; i++)
    {
        Update(); /* 更新指示 */

        if (music != Music_Null)
        {
            break;
        }
        MR_Writer_Holding_Register(slave_addr, MUSIC_ADDR, (unsigned short)Music_Power_On, RT_WAITING_FOREVER); /* 设置音乐 */
        rt_thread_delay(200);                                                                                   /* 更新周期为200ms */
    }

    while (1)
    {
        Update();
        MR_Writer_Holding_Register(slave_addr, ALARM_ADDR, alarm, RT_WAITING_FOREVER); /* 设置警示灯 */
        // MR_Writer_Holding_Register(slave_addr, OUTPUT_ADDR, output, RT_WAITING_FOREVER); /* 设置输出 */
        MR_Writer_Holding_Register(slave_addr, MUSIC_ADDR, music, RT_WAITING_FOREVER); /* 设置音乐 */
        rt_thread_delay(500);                                                          /* 更新周期为500ms */
    }
}

void Update(void)
{
    agv_state = Read_AGV_Move_State(); /* 获取AGV运动状态 */
    switch (agv_state)
    {
    case Move_IDLE: /* 运动空闲 */
        alarm = Alarm_IDLE;
        break;
    case Move_Trigger_Sensor: /* 正常运行时避障已触发 */
        alarm = Alarm_Warning;
        break;
    case Move_Stop_For_Waitting: /* 停车等待,重定义为任务暂停 */
    case Move_Normal:            /* 正常运行 */
    case Move_Over:              /* 当前运动指令结束 */
    case Move_Cancel:            /* 运动任务取消中 */
        alarm = Alarm_Norm;
        break;
    default:
        alarm = Alarm_IDLE;
        break;
    }

    char flag = 0;

    for (int i = 0; i < 4; i++)
    {
        avoid[i] = Avoid_Read(i);
        flag |= ((Sensor_Slow == avoid[i]) || (Sensor_Stop == avoid[i]) || (Sensor_Brake == avoid[i]));
    }

    if (flag)
    {
        music = Music_Avoid_Check;
        Set_Avoid_Check(1);
    }
    else
    {
        Set_Avoid_Check(0);
        if (Move_IDLE == agv_state)
        {
            music = Music_Null;
        }
        else
        {
            music = Music_Normal;
        }
    }
}
